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Technological advances in ad-hoc networking and miniaturization of electro-mechanical 

systems are making possible the use of large numbers of mobile agents (e.g., mobile robots, 

human agents, unmanned vehicles) to perform surveillance, search and rescue, transport and 

delivery tasks in aerial, underwater, space, and land environments. However, the successful 

execution of such tasks often hinges upon accurate position information, which is needed in lower 

level locomotion and path planning algorithms. Common techniques for localization of mobile 

robots are the classical pre-installed beacon-based localization algorithms [[0, fixed feature- 

based Simultaneous Localization and Mapping (SLAM) algorithms Q, and GPS navigation (Si, 

see Fig. for further details. However, in some operations such as search and rescue [01, 

Il5l . environment monitoring [[6l, [|71, and oceanic exploration [|8l, the assumptions required by 

the aforementioned localization techniques include the existence of distinct and static features 

that can be revisited often, or clear line-of-sight to GPS satellites. Such conditions may not 

be realizable in practice, and thus these localization techniques become unfeasible. Instead, 

Cooperative localization (CL) is emerging as an alternative localization technique that can be 
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employed in such scenarios. 


In CL, a group of mobile agents with processing and communication capabilities use 
relative measurements with respect to each other (no reliance on external features) as a feedback 
signal to jointly estimate the poses of all team members, which results in an increased accuracy 
for the entire team. The particular appeal of CL relies on the fact that sporadic access to accurate 
localization information by a particular robot results into a net benefit for the rest of the team. This 
is possible thanks to the coupling that is created through the state estimation process. Another nice 
feature of CL is its cost effectiveness, as it does not require extra hardware beyond the operational 
components normally used in cooperative robotic tasks. In such situations, agents are normally 
equipped with unique identifiers and sensors which enable them to identify and locate other group 
members. To achieve coordination, these agents often broadcast their status information to one 
another. In addition, given the wide and affordable availability of communication devices, CL 
has also emerged as an augmentation system to compensate for poor odometric measurements, 
noisy and distorted measurements from other sensor suites such as onboard IMU systems, see 
e.g., [Hi. 

The idea of exploiting relative robot-to-robot measurements for localization can be traced 
back to [fTOl . where members of a mobile robotic team were divided into two groups, which took 
turns remaining stationary as landmarks for the others. In later developments in ffTTI . where the 
term cooperative localization was also introduced, the necessity for some robots to be stationary 
was removed. Since then, many cooperative localization algorithms using various estimation 
strategies such as Extended Kalman filters (EKE) [fT^ . maximum likelihood liTSll . maximum 
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a posteriori (MAP) [[T4l . and particle filters ifTSl . [IT^ . ifTTl have been developed. Cooperative 
localization techniques to handle system and measurement models with non-Gaussian noises are 
also discussed in [fl^ . lfT9l . 

Although CL is a very attractive concept for multi-robot localization, which does not 
require environmental features or GPS information, it also poses new challenges associated with 
the implementation of such a policy with acceptable communication, memory, and processing 
costs. Cooperative localization is a joint estimation process which results in highly coupled pose 
estimation for the full robotic team. These couplings/cross-correlations are created due to the 
relative measurement updates. Accounting for these coupling/cross-correlations is crucial for both 
filter consistency and also for propagating the benefit of a robot-to-robot measurement update to 
the entire group. In Section “Cooperative localization via EKF” we demonstrate these features 
in detail both through technical and simulation demonstrations. 

A centralized implementation of CL is the most straightforward mechanism to keep an 
accurate account of these couplings and, as a result, obtain more accurate solutions. In a 
centralized scheme, at every time-step, a single device, either a leader robot or a fusion center 
(FC), gathers and processes information from the entire team. Then, it broadcasts back the 
estimated location results to each robot (see e.g., ifT^ . [l20ll l. Such a central operation incurs into 
a high processing cost on the FC and a high communication cost on both FC and each robotic 
team member. Moreover, it lacks robustness that can be induced by single point failures. This 
lack of robustness and energy inefficiency make the centralized implementation less preferable. 

A major challenge in developing decentralized CF (D-CF) algorithms is how to maintain 
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a precise account of cross-correlations and couplings between the agents’ estimates without 
invoking all-to-all communication at each time-step. The design and analysis of decentralized 
CL algorithms, which maintain the consistency of the estimation process while maintaining 
“reasonable” communication and computation costs have been the subject of extensive research 
since the CL idea’s conception. In Section “Decentralized cooperative localization: how to 
account for intrinsic correlations in cooperative localization,” we provide an overview of 
some of the D-CL algorithms in the literature, with a special focus on how these algorithms 
maintain/account for intrinsic correlations of CL strategy. We provide readers a more technical 
example of a D-CL algorithm in the Section “The Interim Master D-CL algorithm: a tightly 
coupled D-CL strategy based on Kalman filter decoupling,” which is a concise summary of the 
solution in [l2n developed by the authors. 

The reader interested on technical analysis and details beyond decentralization for CL can 
find a brief literature guide in “Further Reading.” 

Notations: Before proceeding further, let us introduce our notations. We denote by M,^, Onxm 
(when m = 1, we use 0^) and I„, respectively, the set of real positive definite matrices of 
dimension n x n, the zero matrix of dimension n x m, and the identity matrix of dimension 
n X n. We represent the transpose of matrix A G by A^. The block diagonal matrix of 

set of matrices Ai,..., Ajv is Diag(Ai, • ■ ■ , A^v). For finite sets Vi and V 2 , Vi\V 2 is the set of 
elements in Vi, but not in V2. For a finite set V we represent its cardinality by \V\. In a team 
of N agents, the local variables associated with agent i are distinguished by the superscript i, 
e.g., X* is the state of agent i, x* is its state estimate, and P* is the covariance matrix of its 
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state estimate. We use the term cross-covariance to refer to the eorrelation terms between two 


agents in the eovarianee matrix of the entire network. The eross-eovarianee of the state veetors 
of agents i and j is P^. We denote the propagated and updated variables, say x*, at time-step 
k by x*’(fc) and x*'''(fc), respeetively. We drop the time-step argument of the variables as well 
as matrix dimensions whenever they are elear from the eontext. In a network of N agents, 
p = (p^,..., p^) e d = is ths aggregated veetor of loeal veetors p* G 

Cooperative localization via EKF 

This section provides an overview of a CL strategy that employs an EKF following [[22ll . By 
a close examination of this algorithm, it is possible to explain why accounting for the intrinsic 
cross-correlations in CL is both crucial for filter consistency and key to transmit the benefit 
of an update of a relative robot-to-robot measurement to the entire team. We also discuss the 
computational cost of implementing this algorithm in a centralized manner. 

First, we briefly describe the model considered for the mobile robots in the team. Consider 
a group of N mobile agents with communication, processing and measurement capabilities. 
Depending on the adopted CL algorithm, communication can be in bidirectional manner with a 
fusion center, a single broadcast to the entire team or in multi-hop fashion as shown in Fig. 
i.e., every agent re-broadcasts every received message intended to reach the entire team. Each 
agent has a detectable unique identifier (UID) which, without loss of generality, we assume to 
be a unique integer belonging to the set V = {1,..., A^}. Using a set of so-called proprioceptive 
sensors every agent i G V measures its self-motion, for example by compass readings and/or 
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wheel eneoders, and uses it to propagate its equations of motion. 


x*(fc + 1) = r(x*(A;), u*(fc)) + g\x.\k))r]\k), 


( 1 ) 


where x® G M”*, u® G W^\ and r/® G are, respectively, the state vector, the input vector and the 
process noise vector of agent i. Here, f*(x®, u®) and g®(x®), are, respectively, the system function 
and process noise coefficient function of the agent i G V. The state vector of each agent can be 
composed of variables that describe the robots global pose in the world (e.g. latitude, longitude. 


direction), as well as other variables potentially needed to model the robots dynamics (e.g. 


steering angle, actuation dynamics). The team can consist of heterogeneous agents, nevertheless, 
the collective motion equation of the team can be represented by 


x(fc + 1) = f(x(/c), u(/c)) + g(x(/c))? 7 (fc), 


( 2 ) 



Obviously, if each agent only relies on propagating its equation of motion in ([T]) using 
self-motion measurements, this state estimate grows unbounded due to the noise term ? 7 ®(/c). To 
reduce the growth rate of this estimation error, a CL strategy can be employed. Thus, let every 
agent i G V also carry exteroceptive sensors to monitor the environment to detect, uniquely, the 
other agents j G V in the team and take relative measurements 


Zij{k + 1) = hy(x®(A;),x^(/c)) + v\k), 


(3) 


6 


where Zjj G from them, e.g., relative pose, relative range, relative bearing measurements, or 
both. Here, hjj (x®, x-^) is the measurement model and is the measurement noise of agent i G V. 
Relative-measurement feedback, as shown below, can help the agents improve their localization 
accuracy, though the overall uncertainty can not be bounded (c.f. [l22]|). The tracking performance 
can be improved significantly if agents have occasional absolute positioning information, e.g., via 
GPS or relative measurements taken from a fixed landmark with a priori known absolute location. 
Any absolute pose measurement by an agent i G V, e.g., through intermittent GPS access, is 
modeled by zai^k + 1) = hjj(x®(fc)) + i>®(fc). The agents can obtain concurrent exteroceptive 
absolute and relative measurements. 

Let us assume all the process noises 77® and the measurement noise i G V, are indepen¬ 
dent zero-mean white Gaussian processes with, respectively, known positive definite variances 
Q*(fc) = E[r]'^{k)r]^{ky], IV{k) = E[i/'^{k)u'^{ky] and R*(/c) = E[u'^{k)i>'^{ky]. Moreover, 
let all the sensor noises be white and mutually uncorrelated and all sensor measurements be 
synchronized. Then, the centralized EKF CL algorithm is a straightforward application of EKF 
over the collective motion model of the robotic team @ and measurement model @. The 
propagation stage of this algorithm is 

x‘(fc-f 1) = f(x'''(fc),u(fc)), (4a) 

P'{k + 1) = F(A;)P+(A;)F(A;)^ + G(A;)Q(A;)G(A;)^. (4b) 

where F = Diag(F\ • ■ ■ ,F'^), G = Diag(G^, • • ■ , G^) and Q = Diag(Q^,-- - ,Q'^), with, 
for all z G V, F® = ^f (x®'''(/c), u®(/c)) and G® = ^g(x®'''(/c)). 

If there exists a relative measurement in the network at some given time k + 1, say robot 
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a takes relative measurement from robot b, the states are updated as follows. The innovation of 
the relative measurement and its eovarianee are, respeetively, 

r“ = Zab - ha6(x“'(A; + + 1)), (5a) 

Sa6 = H„fe(A;+l)p-(fc + l)H,b(A; + l)^+R“(fc + l). (5b) 

where (without loss of generality we let a < b) 

pi ... ^ a+l ... ~ 6+1 ... 

iiab{k)= [0 -H+fc) 0 Ubik) 0 ■■■], 

Ha(fc) =-^hafe(x“'(fc),+'(fc)), (6) 

^b{k) = ^hab(x“'(fc),+"(fc)). 

An absolute measurement by a robot a G V ean be proeessed similarly, exeept that in Q, 
Hfe beeomes zero, while in ©■ the index b should be replaeed by a and R“(/c + l) should be 
replaeed by R“(fc + 1). Then, the Kalman filter gain is given by 

K{k + 1) = p-{k + l)Uab{k + iySab~". 

And, finally, the eolleetive pose update and eovarianee update equations for the network are: 

x'''(fc + l) =x’(/c+l) + K(/c + l)r“, (7a) 

P+(fc + l) =p-(A: + l)-K(fc + l)S,feK(fc + l)^. (7b) 

Beeause K(fc+l)SafeK(fc+l)^ is a positive semi-definite term, the update equation ( |7b| ) clearly 
shows that any relative measurement update results in a reduction of the estimation uncertainty. 

To explore the relationship among the estimation equations of each robot, we express the 

aforementioned collective form of the EKF CL in terms of its agent-wise components, as shown 
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in Algorithm 


k;, ,K 


N 


, where 


Here, the Kalman filter gain is partitioned into K = 

K, e is the portion of the Kalman gain used to update the pose estimate of the agent 

f G V. To proeess multiple synehronized measurements, sequential updating (e.f. for example 
Ch. 31. [|2^ 1 is employed. 


Algorithm elearly showeases the role of past eorrelations in a CL strategy. First, observe 
that, despite having decoupled equations of motion, the source of the coupling in the propagation 
phase is the cross-covariance equation ( |16c| ). Upon an incidence of a relative measurement 
between agents a and b, this term becomes non-zero and its evolution in time requires the 
information of these two agents. Thus, these two agents have to either communicate with each 
other all the time or a central operator has to take over the propagation stage. As the incidences 
of relative measurements grow, more non-zero cross-covariance terms are created. As a result, 
the communication cost to perform the propagation grows, requiring the data exchange all the 
time with either a Fusion Center (FC) or all-to-all agent communications, even when there is 
no relative measurement in the network. The update equations ( [T8] ) are also coupled and their 
calculations need, in principle, a FC. The next observation regarding the role of the cross¬ 
covariance terms can be deduced from studying the Kalman gain equation ( [T9| ). As this equation 
shows, when an agent a takes a relative measurement from agent b, any agent whose pose 
estimation is correlated with either of agents a and b in the past, (i.e., P’ft(/c-|- 1) and/or Pl^{k + 
1) are non-zero) has a non-zero Kalman gain and, as a result, the agent benefits from this 
measurement update. The same is true in the case of an absolute measurement taken by a 
robot a. 
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The following simple simulation study demonstrates the signifieanee of maintaining an 
aeeurate aeeount of eross-eovarianee terms between the state estimates of the team members. 
We eonsider a team of 3 mobile robots moving on a flat terrain whose equations of motion in 
a fixed reference frame, for i G {1,2,3}, are modeled as 

x^(k + 1) =x\k) + V^{k) cos(0(fc)) St, 
y\k + 1) =y\k) + V\k) sin(0(A;)) St, 

(j)\k + 1) =(j)\k) + u{k) St, 

where V^{k) and a;*(/c) are true linear and rotational velocities of robot i at time k and St is the 

stepsize. Here, the pose vector of each robot is x* = [x\ ?/*, 0*]^. Every robot uses odometric 

sensors to measure its linear V,^{k) = V^{k) + rfy{k) and rotational u:]^{k) = u^{k) + r]l{k), 

velocities, where r]y and are their respective contaminating measurement noise. The 

standard deviation of yy{k), i G {1,2,3}, is 0.1V^{k), while the standard deviation of 77 ^ 

is 1 deg/s, for robot 1 and robot 2, and 0.5 deg/s for robot 3. Robots {1,2,3} can take 

relative pose measurements from one another. Here, we use standard deviations of, respectively 

(0.05 m, 0.05 m, 1 deg/s), (0.05 m, 0.05 m, 2 deg/s), (0.07m, 0.07m, 1.5 deg/s) for measurement 

noises. Assume robot 1 can obtain absolute position measurement with a standard deviation of 

(0.1m, 0.1m) for the measurement noise. Figure [^demonstrates the x-coordinate estimation error 

(solid line) and the 3a error bound (dashed lines) of these robots when they (a) only propagate 

their equations of motion using self-motion measurements (black plots), (b) employ an EKE 

CE ignoring past correlations between the estimations of the robots (blue plots), (c) employ 

an EKE CE with an accurate account of past correlations (red plots). As this figure shows, 
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employing a CL strategy improves the localization accuracy by reducing both the estimation error 
and its uncertainty. However, as plots in blue show, ignoring the past correlations (here cross¬ 
covariances) among the robots state estimates results in overly optimistic estimations (notice the 
almost vanished 3a error bound in blue plots while the solid blue line goes out of these bounds, 
an indication of inconsistent estimation). In contrast, by taking into account the past correlations 
(see red plots), one sees a more consistent estimation. 

Figure also showcases the role of past cross-covariances to expand the benefit of a 
relative measurement between two robots, or of an absolute measurement by a robot to the 
rest of the team. For example consider robot 2. In the time interval [10, 90] seconds, robot 1 is 
taking a relative measurement from robot 2. As a result, the state estimation equation of robot 1 
and robot 2 are correlated, i.e, the cross-covariance term between these two robots is non-zero. 
Therefore, in the time interval [90,110] seconds, when the estimation update is due to the relative 
measurement taken by robot 3 from robot 1, the estimation of robot 2 is also improved (see red 
plots.) In the time interval [190, 240] seconds, when the estimation update is due to the absolute 
measurement taken by robot 1, robot 2 and 3 also benefit from this measurement update due 
to past correlations (see the red plots.) Figure shows the trajectories of the robots when they 
apply EKF CL strategy. For more enlightening simulation studies, we refer the interested reader 
to [EH. 
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Decentralized cooperative localization: how to account for intrinsic 


correlations in cooperative localization 

Based on the observations that 

(a) past eorrelations eannot be ignored, 

(b) they are useful to inerease the loealization aeeuraey of the team, 

(e) the eoupling that the eorrelations ereate in the state estimation of team members is the main 
ehallenge in developing a deeentralized eooperative loealization algorithm, 

one ean find, regardless of the teehnique, two distinet trends in the design methodology of deeen¬ 
tralized eooperative loealization algorithms in the literature. We term these as “loosely eoupled” 
and “tightly eoupled” deeentralized eooperative loealization (D-CL) strategies respeetively (see 
Fig. 

In the loosely eoupled D-CL methodology, only one or both of the agents involved in a 

relative measurement update their estimates using that measurement. Here, an exaet aeeount of 

the “network” of eorrelations (see Fig. due to the past relative measurement updates is not 

aeeounted for. However, in order to ensure estimation eonsisteney, some steps are taken to fix 

this problem. Examples of loosely eoupled D-CL are given in [jH, ll25l . [|2^ . [l27l and f[28l . In 

the algorithm of [[8l|, only the agent obtaining the relative measurement updates its state. Here, in 

order to produee eonsistent estimates, a bank of extended Kalman filters (EKFs) is maintained 

at eaeh agent. Using an aeeurate book-keeping of the identity of the agents involved in previous 

updates and the age of sueh information, eaeh of these filters is only updated when its propagated 
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state is not correlated to the state involved in the current update equation. Although this technique 
does not impose a particular communication graph on the network, the computational complexity, 
the large memory demand, and the growing size of information needed at each update time are 
its main drawbacks. In the approach [l25ll it is assumed that the relative measurements are in 
the form of relative pose. This enables the agent taking the relative measurement to use its 
current pose estimation and the current relative pose measurement to obtain and broadcast a 
pose and the associated covariance estimation of its landmark agent (the landmark agent is the 
agent the relative measurement is taken from). Then, the landmark agent uses the Covariance 
Intersection method (see [|^ . [[30l) to fuse the newly acquired pose estimation with its own 
current estimation to increase its estimation accuracy. Covariance Intersection for D-CL is also 
used in [l26ll for the localization of a group of space vehicles communicating over a fixed ring 
topology. Here, each vehicle propagates a model of the equation of motion of the entire team 
and, at the time of relative pose measurements, it fuses its estimation of the team and of its 
landmark vehicle via Covariance Intersection. Another example of the use of split Covariance 
Intersection is given in [|27l . for intelligent transportation vehicles localization. Even though 
the Covariance Intersection method produces consistent estimations for a loosely coupled D- 
CL strategy, this method is known to produce overly conservative estimates. Another loosely- 
coupled CL approach is proposed in [|2^ . which uses a Common Past-Invariant Ensemble Kalman 
pose estimation filter of intelligent vehicles. This algorithm is very similar to the decentralized 
Covariance Intersection data fusion method described above, with the main difference that it 
operates with ensembles instead of with means and covariances. Overall, the loosely coupled 
algorithms have the advantage of not imposing any particular connectivity condition on the team. 
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However, they are eonservative by nature, as they do not enable other agent in the network to 
fully benefit from measurement updates. 

In the tightly eoupled D-CL methodology, the goal is to exploit the “network” of 
eorrelations ereated aeross the team (see Fig. [^, so that the benefit of the update ean be 
extended beyond the agents involved in a given relative measurement. However, this advantage 
comes at a potentially higher computational, storage and/or communication cost. The dominant 
trend in developing decentralized cooperative localization algorithms in this way is to distribute 
the computation of components of a centralized algorithm among team members. Some of the 
examples for this class of D-CL is given in OTl . [|^ . IfT^ . [[32l, [|33l|. In a straightforward 
fashion, decentralization can be conducted as a multi-centralized CL, wherein each agent 
broadcasts its own information to the entire team. Then, every agent can calculate and reproduce 
the centralized pose estimates acting as a fusion center OTl . Besides a high-processing cost 
for each agent, this scheme requires all-to-all agent communication at the time of each 
information exchange. A D-CL algorithm distributing computations of an EKF centralized CL 
algorithm is proposed in [l22l . To decentralize the cross-covariance propagation, [l22l uses a 
singular-value decomposition to split each cross-covariance term between the corresponding 
two agents. Then, each agent propagates its portion. However, at update times, the separated 
parts must be combined, requiring an all-to-all agent communication in the correction step. 
Another D-CL algorithm based on decoupling the propagation stage of an EKF CL using 
new intermediate variables is proposed in [l2TI . But here, unlike [l22l . at update stage, each 
robot can locally reproduce the updated pose estimate and covariance of the centralized EKF 

after receiving an update message only from the robot that has made the relative measurement. 
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Subsequently, ffT4l . [|^ present D-CL strategies using maximum-a-posteriori (MAP) estimation 
proeedure. In the former, eomputations of a eentralized MAP is distributed among all the 
team members. In the latter, the amount of data required to be passed between mobile agents 
in order to obtain the benefits of eooperative trajeetory estimation locally is reduced by 
letting each agent to treat the others as moving beacons whose estimate of positions is only 
required at communication/measurement times. The aforementioned techniques all assume that 
communication messages are delivered, as prescribed, perfectly all the time. A D-CL approach 
equivalent to a centralized CL, when possible, which handles both limited communication ranges 
and time-varying communication graphs is proposed in [1^ . This technique uses an information 
transfer scheme wherein each agent broadcasts all its locally available information to every agent 
within its communication radius at each time-step. The broadcasted information of each agent 
includes the past and present measurements, as well as past measurements previously received 
from other agents. The main drawback of this method is its high communication and memory 
cost, which may not be affordable in applications with limited communication bandwidth and 
storage resources. 

The Interim Master D-CL algorithm: a tightly coupled D-CL strategy based 

on Kalman filter decoupling 

Because of its recursive and simple structure, the EKF is a very popular estimation strategy. 
However, as discussed in Section “Cooperative localization via EKF,”a naive decentralized 
implementation of EKE requires an all-to-all communication at every time-step of the algorithm. 
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In this section, we describe how by exploiting a special pattern in the propagation estimation 
equations, [l22l and [|2T| proposed tightly coupledexact decentralized implementations of EKF 
for CL with reduced communication workload per agent. Here, what we mean by “exact” is 
that if these decentralized implementations are initialized the same as a centralized EKF, they 
produce the same state estimate and the associated state error covariance of the centralized filter. 
Our special focus in this section is on the Interim Master D-CL of [12111 . 

The Interim Master D-CL algorithm and the algorithm of [|22l are developed based on 
the observation that, in localization problems, we are normally only interested in the explicit 
value of the pose estimate and the error covariance associated with it, while cross-covariance 
terms are only required in the update equations. Such an observation promoted the proposal 
of the implicit tracking of cross-covariance terms by splitting them into intermediate variables 
that can be propagated locally by the agents. Then, cross-covariance terms can be recovered by 
putting together these intermediate variables at any update incidence. Let the last measurement 
update be in time-step k and assume that for m subsequent and consecutive steps no relative 
measurement incidence takes place among the team members, i.e., no intermediate measurement 
update is conducted in this time interval. In such a scenario, the propagated cross-covariance 
terms for these m consecutive steps are given by 

Y-..{k + l) = Y\k + l-l) ••• F\k)Ptjik)F^{k)^ F\k + l-lf, I e {!,■■■ ,m}, 

( 8 ) 

for z G V and j G V\{z}. That is, at each time step after k, the propagated cross-covariance 

term is obtained by recursively multiplying its previous value by the Jacobian of the system 

function of agent i on the left and by the transpose of the Jacobian of the system function 
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of agent j at that time step on the right. Based on this observation, Roumeliotis and Bekey 
in [l22ll proposed to deeompose the last updated cross-eovariance term Pj(/c) between any 
agent i and any other agent j of the team into two parts (for example using the singular value 
decomposition technique). Then, agent i will be responsible for propagating the left portion 
while agent j propagates the right portion. Note that, as long as there is no relative measurement 
among team members, each agent can propagate its portion of the cross-covariance term locally 
without a need of communication with others. This was an important result, which led to a 
fully decentralized estimation algorithm during the propagation cycle. However, in the update 
stage, all the agents needed to communicate with one and other to put together the split cross¬ 
covariance terms and proceed with the update stage. The approach to obtain Interim Master D- 
CL, which is outlined below, is also based on the special pattern that the cross-covariance 
propagation equations have in That is, we also remove the explicit calculation of the 
propagated cross-covariance terms by decomposing them into the intermediate variables that 
can be propagated by agents locally. However, this alternative decomposition allows every agent 
to update its pose estimate and its associated covariance in a centralized equivalent manner, using 
merely an scalable communication message that is received from the team member that takes 
the relative measurement. As such, the Interim Master D-CL algorithm removes the necessity of 
an all-to-all communication in the update stage and replaces it with propagating a constant size 
communication message that holds the crucial piece of information needed in the update stage. 

In particular, we observe that Pij{k -f /) in @ is composed of the following 3 parts: (a) 

F\k + I — 1) ■ • • F\k) which is local to agent i, (b) the Pfjik) that does not change unless 

there is relative measurement among the team members, and (c) • • • F^{k +1 — 
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which is local to agent j. Motivated by this observation, we propose to write the propagated 


cross-covariances (1^) as: 


+ 1) = + l)U,,{k)^^{k + 1)^, A: e {0,1, 2, • • ■ }, 


(9) 


where G for alH G V, is a time-varying variable that is initialized at $*(0) = I„i 

and evolves as: 


^\k + 1) = F'\k)^\k), /c G {0,1, 2, • ■ ■ }, 


( 10 ) 


(it is interesting to notice the resemblance of ([T0|) and the transition matrix for discrete-time 


systems) and Yiij G for i, j G V and i ^ j, which is also a time-varying variable that is 


initialized at njj(O) = O^ixnJ - When there is no relative measurement at time A: -f 1, Q results 
into Yiij{k -f 1) = Yiij{k). However, when there is a relative measurement among the team 
members must be updated. Next, we derive an expression for (A: -f 1) when there is a 
relative measurement among team members at time A: -f 1, such that at time A: -f 2 one can write 
Pij(A: -f 2) = $*(A: -f 2)Ylij{k + l)^^{k + 2)^. For this, notice that the update equations ( [TT] ) 
and ( [T9| ) of the centralized CL algorithm can be rewritten by replacing the cross-covariance terms 
by @ (recall that in the update stage, we are assuming that robot a has taken measurement robot 
robot b): 



K, = ^\k + i)riSakK tev, 

18 






where 

= tev\{a,b}, (12a) 

r, = - ($“)-ip“-HJ) Sab ^ (12b) 

r,= Sab'^. (12e) 

Generally, F*(/c) is invertible for all fc > 0 and i eV. Therefore, ^\k), for all fc > 0 and i eV, 
is invertible. 


Next, for i ^ j and i,j E V, we ean write the eross-eovarianee terms (18e) as: 


P+(fc + 1) = P.^{k + 1)-K, S,,Kj 

= ^\k + i)n,j{k)^^{k + If-{^\k + i)r*s,b-^) Sab {^^{k + i)rjSak'^y 

= ^\k + i){n,,{k) - r,r/)^^{k + 1)^. 


Let us propose 


P^ij{k + 1) — — PiPj • 


Then, the eross-eovarianee update (18e) ean be rewritten as: 


Pj(fc + 1) = ^\k + 1) Ui.ik + 1) ^^{k + 1)^. 


(13) 


Therefore, at time k + 2, the propagated eross-covariances terms for i ^ j and i,jEV are: 

P;j{k + 2) = F‘(4 + 1) Pj(4 + 1) F-'(4 +1)-^ 

= F\k + l)^\k + l)n,j{k + l)^^{k + l^F^ik + 1)^ 

= ^\k + 2)nij{k + i)^\k + 2y. 
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In short, we can rewrite the propagated and the updated cross-covariance terms of the centralized 
EKF CL as, respectively, @ and ( fT3| ) for all k G {0,1, • ■ ■} where the variables $*(/c)’s and 
Hij’s, evolve according to, respectively, and 


Yl.ij{k -f 1) 


' 

Yiij{k)^ no relative measurement at A; -f 1, 

< 

Yiij{k) — Fj rj, otherwise, 


(14) 


for j G V and i ^ j. 


Next, notice that we can write the updated state estimate and covariance matrix in the new 
variables as follows, for z G V, 

x'+(fc + l) =x*'(A; + l) + $'(A; + l)r,f“, (15) 

P*+(A; + 1) = P*-(A; + 1) - $*(fc + i)ri rj^\k + 1)T 

where f“ = Sa^5r“. 

Using the alternative representations @, ( [T3| ), and ( [T5] ) of the EKF CL, the decentralized 
implementation Interim MasterD-CL is given in Algorithm]^ We develop the Interim MasterD- 
CL algorithm by keeping a local copy of Hij’s at each agent z G V, i.e., FI*; for all j G V\{A^} 
and I G {j' + 1, • ■ ■ ,A^}-because of the symmetry of the covariance matrix we only need 
to save, e.g., the upper triangular part of this matrix. For example, for a group of = 4 
robots, every agent maintains a copy of 11 ^ 4 , Fl^g, 1124, ^ 34 }. During the algorithm 

implementation, we assume that if FI*; is not explicitly maintained by agent z, the agent 
substitutes the value of (FIJj)^ for it. 

In Interim Master D-CL, every agent z G V initiali z es its own state estimate x*'''(0), 
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the error eovarianee matrix P*'''(0), $*(0) = I„i, and its loeal eopies n*;(0) = Onixnh for 




j G V\{iV} and I G {j' + 1, ■ ■ ■ ,iV}; see ( |20| ). At propagation stage, every agent evolves its 
local state estimation, error covariance and $*, according to, respectively, ( |16a[ ), ( |16b| ), ( fTO] ); 
see ( [2T| ). At every time step, when, there is no exteroceptive measurement in the team, the 
local updated state estimates and error covariance matrices are replaced by their respective 
propagated counterparts, while n*;’s, to respect ( pA) ), are kept unchanged; see ( |22l ). When there 
is a robot-to-robot measurement, examining (|^, ( [5a| ), ( [TT| ), ( |12b[ ) and ( |12c| ) shows that agent 
a, the robot that made the relative measurement, can calculate these terms using its local 11*; 
and acquiring x^"(fc + l) G M**\ $^(/c + l) G M**'’^***’, and P^’(fc + 1) G see ( |2^ and ( [24| ). 
Then, agent a can assume the role of the interim master and issue the update terms for other 
agents in the team; see ( [25] ). Using this update message and their local variables, then each agent 
i G V can compute ( |12a| ) and use it to obtain its local state updates of ( [T5| ) and ( pA] ); see pT] ). 
Figure demonstrates the information flow direction between agent while implementing the 
Interim Master D-CL algorithm. 


The inclusion of absolute measurements in the Interim Master D-CL is straightforward. 
The agent making an absolute measurement is an interim master that can calculate the update- 
message using only its own data and then broadcast it to the team. Next, observe that the 
Interim Master D-CL algorithm is robust to permanent agent dropouts from the network. The 
operation only suffers from a processing cost until all agents become aware of the dropout. Also, 
notice that an external authority, e.g., a search-and-rescue chief, who needs to obtain the location 
of any agent, can obtain this location update in any rate (s)he wishes to by communicating with 
that agent. This reduces the communication cost of the operation. 
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The Interim Master D-CL algorithm works under the assumption that the message from 
the agent taking the relative measurement, the interim master, is reaehed by the entire team. 
Any eommunieation failure results in a mismateh between the loeal copies of at the agents 
receiving and missing the communication message. The readers are referred to ll34l where the 
authors present a variation of Interim Master D-CL which is robust to intermittent communication 
message dropouts. Such guarantees in [IM1 are provided by replacing the fully decentralized 
implementation with a partial decentralization where a shared memory stores and updates the 
Llij s. 

Complexity analysis 


For the sake of an objective performance evaluation, a study of the computational 
complexity, the memory usage, as well as communication cost per agent per time-step of the 
Interim Master D-CL algorithm in terms of the size of the mobile agent team N is provided 
next. At the propagation state of the Interim Master D-CL algorithm, the computations per agent 
are independent of the size of the team. However, at the update stage, for each measurement 


update, the computation of every agent is of order A^(A^ —1)/2 due to P3c| ). As multiple relative 
measurements are processed sequentially, the computational cost per agent at the completion of 
any update stage depends on the number of the relative measurements in the team, henceforth 
denoted by N^. Then, the computational cost per agent is 0{Nz x N‘^), implying a computational 
complexity of order 0{N*) for the worst case where all the agents take relative measurement 
with respect to all the other agents in the team, i.e., = N{N — 1). The memory cost per agent 

is of order 0{N‘^) which, due to the recursive nature of the Interim Master D-CL algorithm. 
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is independent of N^. This eost is eaused by the initialization ( [20| ) and update equation ( |33e| ), 
whieh are of order N{N — l)/2. 


For the analysis of the eommunieation eost, let us eonsider the ease of a multi-hop 

eommunieation strategy. The Interim Master D-CL requires eommunieation only in its update 

stage, where landmark robots should broadcast their landmark message to their respective master, 

and every agent should re-broadcast any update-message it receives. Let Nr be the number of 

the agents that have made a relative measurement at the current time, i.e.. Nr < N is the 

number of current sequential interim masters. These robots should announce their identity and the 

number of their landmark robots to the entire team for sequential update cuing purpose, incurring 

a communication cost of order Nr per robots. Next, the team will proceed by sequentially 

processing the relative measurements. Every agent can be a landmark of Na < Nr agents and/or 

a master of iVf, < (A^ — 1) agents. The updating procedure starts by a landmark robot sending 

its landmark-message to its active interim master, resulting in a total communication cost of 

0{Na) per landmark robot at the end of update stage. Every active interim master should pass 

an update message to the entire team, resulting in a total communication cost of 0{Nb) per robot. 

Because there are Nr masters, at the end of the update stage, every robot incurs a communication 

cost of 0{Nr X Nb) to pass the update messages. Because Na,Nr < Nr x Nb < N^, the total 

communication cost at the end of the update stage is of order 0{Nz) per agent, implying a worst 

case broadcast cost of 0{N‘^) per agent. If the communication range is unbounded, the broadcast 

cost per agent is 0(max{A"fe, Na}), with the worst case cost of order 0{N). The communication 

message size of each agent in both single or multiple relative measurements is independent of 

the group size N. As such for the worst case scenario the communication message size is of 
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order 0(1). 


The results of the analysis above are summarized in Table and are eompared to those 
of a trivial deeentralized implementation of the EKF for CL (denoted by T-D-CL) in whieh 
every agent i G V at the propagation stage eomputes (fT^-using the broadeasted F^(fc) from 


every other team member j G V\{i}-and at the update stage computes ( pO] ) and ([T8])-using 
the broadcast (a, b, r“, Sab, H^, H;,, R“, P“‘, P^‘) from agent a that has made relative 
measurement from agent b. Agent a calculates Sab, by requesting (x^’, P^‘) from agent 

b. We assume that multiple measurements are processed sequentially and that the communication 
procedure is multi-hop. Although the overall cost of the T-D-CL algorithm is comparable with 
the Interim Master D-CL algorithm, this implementation has a more stringent communication 
connectivity condition, requiring a strongly connected digraph topology (i.e., all the nodes on 
the communication graph can be reached by every other node on the graph) at each time-step, 
regardless of whether there is a relative measurement incidence in the team. As an example, 
notice that the communication graph of Lig. [^is not strongly connected and as such the T-D-CL 
algorithm can not be implemented whereas the Interim Master D-CL algorithm can be. Recall 
that the Interim Master D-CL algorithm needs no communication at the propagation stage and it 
only requires an existence of a spanning tree rooted at the agent making the relative measurement 
at the update stage. Finally, the Interim Master D-CL algorithm incurs less computational cost 
at the propagation stage. 

Algorithm [^presents an alternative Interim Master D-CL implementation where, instead of 
storing and evolving Il/j’s of the entire team, every agent only maintains the terms corresponding 
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to its own cross-covariances; see (|28|) and ([29l). For example in a team of = 4, robot 1 


maintains {nl„n!„nl,}, robot 2 maintains { 1121 , 1123 , 1124 }, ete. However, now the interim 
master a needs to acquire the n^j’s from the landmark robot b and ealeulate and broadcast Fj, 
i G V to the entire team; see ( [30l ), ( [3T] ) and In this alternative implementation, the proeessing 
and storage eost of every agent is redueed from O(iV^) to 0{N), however the eommunieation 
message size is inereased from 0(1) to 0{N). 


Tightly coupled versus loosely coupled D-CL: a numerical comparison study 


The Interim Master D-CL falls under the tightly eoupled D-CL classifieation. Fig. |7] 
demonstrates the positioning accuraey (time history of the root mean square error (RMSE) 
plot for 50 Monte Carlo simulation runs) of this algorithm versus the loosely eoupled EKF and 
Covarianee-Interseetion based algorithm of ll25l in the following seenario. We eonsider the 3 
mobile robots employed in the numerieal example of Seetion “Cooperative loealization via EKF” 
with motion as described in that seetion. For the sensing seenario here, we assume that, starting 
at f = 10 seeonds, robot 3 takes persistent relative measurements alternating every 50 seeonds 
from robot 1 to robot 2 and viee versa. As expeeted, the tightly eoupled Interim Master D-CL 
algorithm produees more aeeurate position estimation results than those of the loosely eoupled 
D-CL algorithm of [|25l (similar results ean be observed for the heading estimation aeeuraey, 
which is omitted here for brevity). 

In the algorithm of ||251 . every robot keeps an EKF estimation of its own pose. When a 
robot takes a relative pose measurement from another robot (let us eall this robot the interim 
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master here as well), it acquires the current position estimation and the corresponding error 
covariance of the landmark robot. Then, it uses these along with its own current estimation 
and the current relative measurement to extract a new state estimation and the corresponding 
error covariance for the landmark robot. After this, the interim master robot transmits these 
new estimates to the landmark robot which uses the Covariance Intersection method to fuse 
them consistently to its current pose estimate. It is interesting to notice that in this particular 
scenario, even though robot 3 has been taking all the relative measurements, it receives no benefit 
from such measurements, because only the landmark robots are updating their estimations. Even 
though the positioning accuracy of algorithm f[25l is lower, it only requires 0(1) computational 
cost per agent as compared to the 0{N‘^) cost of the Interim Master D-CL algorithm. However, 
it also requires more complicated calculations to perform Covariance Intersection fusion. If we 
assume that the communication range of each agent covers the entire team, then interestingly 
the communication cost of these two algorithms is the same as both use an 0(1) landmark 
and update messages. However, if the communication range is bounded, the loosely coupled 
algorithm of ll25l offers a more flexible and cost effective communication policy. 

Conclusions 

Here, we presented a brief review on Cooperative Localization as an strategy to increase the 

localization accuracy of team of mobile agents with communication capabilities. This strategy 

relies on use of agent-to-agent relative measurements (no reliance on external features) as a 

feedback signal to jointly estimate the poses of the team members. In particular, we discussed 

challenges involved in designing decentralized Cooperative Localization algorithms. Moreover, 
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we presented a deeentralized eooperative loealization algorithm that is exaetly equivalent to the 
eentralized EKF algorithm of . In this deeentralized algorithm, the propagation stage is fully 
deeoupled i.e., the propagation is a local calculation and no intra-network communication is 
needed. The communication between agents is only required in the update stage when one agent 
makes a relative measurement with respect to another agent. The algorithm declares the agent 
made the measurement as interim master that can, by using the data acquired from the landmark 
agent, calculate the update terms for the rest of the team and deliver it to them by broadcast. 
Future extensions of this work includes concern handling message dropouts and asynchronous 
measurement updates. 
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Algorithm 1 EKF CL (centralized) 

Require: Initialization {k = 0): For i G V, the algorithm is initialized at 


x»+(o)eM"’ p*+(o)eM„.,p+,(o) = jev\{i}. 

Iteration k 

1: Propagation: for i S V, the propagation equations are: 

x®'(fc + l) = f®(x®‘''(fc), u®(A:)), (16a) 

P»-(fc + l)= F»(A:)P»+{fc)F*(fc)T+G'(fc)Q*(fc)G‘(fc)T (16b) 

p-^(fc + l)=F*(fc)P+.(fc)F^(fc)T ieV\{*}. (16c) 

2: Update: While there are no relative measurements no update happens, i.e., 

x+(fc + 1) = x'(fc + 1), P+(fc + 1) = P'(fc + 1). 


When there is a relative measurement at time-step for example robot a makes a relative measurement of robot b, the update proceeds 

as below. The innovation of the relative measurement and its covariance are, respectively, 

r“ = Zab - hai,(x“'(fc + l),x'’'(fc + 1)), 

and 


S<,i, = R“(fc + 1) + Haik + l)P“-(fc + l)Ha(fc + 1)^ + Hi,(fc + l)P'’-(fc + l)Hb{k + 1)^ 

- HbCfc + l)p-6„(fc + l)H,(fc + 1)T _ H„(fc + l)Plb{k + l)Ub{k + 1)T. (17) 

The estimation updates for the centralized EKF are: 

x'+(A:+l)=x*'(fc+l) + Ki(fc+l)r“(A:+l), (18a) 

P*+(fc+l)=P*-(fc + l)-Ki(fc + l)S„6(fc+l)Ki(fc+l)T (18b) 

P+(fc+l)=P:/fc + l)-Ki(fc + l)S,6(fc + l)Kj(fc+l)| (18c) 

where i GV, j G V\{ 2 } and 

K^ = (P:i,(fc + l)H^ - PlJk + l)Hr)S,6-1. (19) 

3: k i — /c -|- 1 
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Algorithm 2 Interim Master D-CL 

Require: Initialization (fc = 0): Every agent i E V initializes its filter at 


x*+(o) eiR"\p*+(o) eM„i, #*(o) = i„i, 


= jev\{N}, ie{j + i,--- ,N}. 


( 20 ) 


Iteration k 

1: Propagation: Every agent z E V propagates the variables below 


x>-(A:+l) = r(x*+(fc),u*(fc)), P‘'(fc+l) = F‘(fc)P‘+(fc)F*(fc)T+GXfc)Q*(fc)G‘(fc)T ^*(fc + l) =F'(A:)#'(fc). (21) 


2: Update: while there are no relative measurements in the network, every agent z E V updates its variables as: 

x'+(fc + i) = x'-(fc + i), p*+(fc + i) = P‘-(fc + i), n» (fc + i) = n;.(fc), iev\{(V}, + ,Af}. (22) 


If there is an agent a that makes a measurement with respect to another agent 6, then agent a is declared as the interim master and acquires 
the following information from agent b: 


landmark-message = + 1), + 1), P^~(k + 1)^ . 


Agent a makes the following calculations upon receiving the landmark-message: 
r“ = Zab - 

S,i, = R“ + H,P“-H^ + H([ P'’-Hi, - 

F, = - (#“)-ip“-Hj)S,6-5, Pi, = (($^)-ip‘’-H^ - 


(23) 

(24a) 

(24b) 

(24c) 


update-message= {^a,b,r°^,ra,rt,^ Sab 2 , Sab 

Every agent i S V, upon receiving the update-message, first calculates, Vy S V\{a, b}, using information obtained at k: 


where Ha(fc + 1) = Ha(x“',x*’') and Hb(A: + 1) = Hb(x“',x*’') are obtained using 

The interim master passes the following data, either directly or indirectly (by message passing), to the rest of the agents in the network: 

(25) 


(26) 

(27a) 

(27b) 

(27c) 


Pf = n},#" H, s,b-^ - ' H, S,b-5, 


and then updates the following variables: 

3e+(fc+l) = x“-(fc + l) + #*(fc+l) Fi f“, 

F»+(fc+i) = p-((=+i)-^'(fc+i)PiPT^Xfc+ir- 
iP,,(fc+i) = n,Xfc)-P,P,"^, 2ev\{7V},ie{i + i,--- ,7V}. 

3: k i — k 1. 
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Algorithm 3 Alternative Interim Master D-CL (larger eommunieation message size in favor of 
lower eomputation and storage cost per agent) 

Require: Initialization (fc = 0): Every agent i E V initializes its filter at 

x*+(o) e R-’,P‘+(o) e M„,, #'(o) = n)^-(o) = j e v\{i}. ( 28 ) 


Iteration k 

1: Propagation: Every agent i E V propagates the variables below 


x»-(A:+l) = r(x*+(fc),u*(fc)), P*'(fc+l) = F‘(fc)P‘+(A:)F‘(fc)T+GXfc)QXfc)GXfc)T ^‘(fc + 1) =F'(A:)#*(fc). (29) 


2: Update: while there are no relative measurements in the network, every agent 2 E V updates its variables as: 

x‘+(fc + i) = x*-(fc + i), p‘+(fc + i) = p‘-(fc + i), n)^(fc + i) = n)j(fc), iev\{i}. 

If there is an agent a that makes a measurement with respect to another agent 6, then agent a is declared as the interim master and acquires 
the following information from agent b: 

landmark-message = + 1), + 1), + 1), where j E (30) 

Agent a makes the following calculations upon receiving the landmark-message: 

r“ = Zab - ha6(x“‘,x*''), (31a) 

S,i, = R“ + H,P“-hJ + P*’-H6 - (31b) 

F, = - ($“)-ip“-Hj)S,(,-5, Fi, = (($'’)-ipb-H([ - (31c) 

F,-= i6V\{a,fe}, (31d) 

where Ha(/c + 1) = Ha(x“', x^') and + 1) = H^)(x“', x^') are obtained using 

The interim master passes the following data, either directly or indirectly (by message passing), to the rest of the agents in the network: 


update-message = &, r“, Fi, ■ • • , Fjv^ ■ 


Eveiy agent 2 E V, upon receiving the update-message, updates the following variables: 

i^+(fc + l) = x^-(/c+l) + ^^(A:+l) r, r", 

F+(fc+i) = p^"(^+i)-^*(^+i)r*rj^XA:+i)^, 
itiiik+i) = itiJk)-r,Tj, j e v\W. 


(32) 

(33a) 

(33b) 

(33c) 


3: k i — k 1- 
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TABLE I: Complexity analysis per agent of the Interim Master D-CL algorithm (denoted by 
IM-D-CL) eompared to that of the trivial decentralized implementation of EKE for CE (denoted 
by T-D-CE) introduced in Subsection 



Computation 

Storage 

Broadcast* 

Message Size 

Connectivity 

Algorithm 

IM-D-CL 

T-D-CL 

IM-D-CL 

T-D-CL 

IM-D-CL 

T-D-CL 

IM-D-CL 

T-D-CL 

IM-D-CL 

T-D-CL 

Propagation 

0(1) 

OiN^) 

0(iV2) 

0{N^) 

0 

0{N) 

0 

0(1) 

None 

Strongly 

connected 

digraph 

Update per Nz 

relative measur. 

0{N^xN^) 

OiN^xN^) 

0{N^) 

0(N'^) 

0{N:,) 

0(N.) 

0(1) 

0(1) 

interim mas¬ 
ter can reach 

all the agents 

Overall worst case 

0{N*) 

OiN*) 

0{N^) 

0{N'^) 

0{N'^) 

0{N^) 

0(1) 

0(1) 


Broadcast cost is for multi-hop communication. If the communication range is unbounded, the broadcast cost per agent is 0(max{A^5, Na}) 

with the worst cost of 0{N). 
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Beacon-based localization 


SLAM [35] 


GPS navigation 


Legend) Tme agent t Estimated agent o True landmark Estimated landmark 

GPS satellite i - Exteroceptive measurement -Actual robot path 


Figure 1: Schematic representation of common probabilistic localization techniques for mobile platforms; In 
beacon-based localization, the map of the area is known and there are pre-installed beacons or landmarks with 
known locations and identities. By taking relative measurements with respect to these landmarks, the mobile agents 
can improve their localization accuracy. For operations where a priori knowledge about the environment is not 
available, but nevertheless, the environment contains fixed and distinguishable features that agents can measure, 
SLAM is normally used to localize the mobile agents. SLAM is a process by which a mobile agent can build a map 
of an environment and at the same time use this map to deduce its location. On the other hand, GPS navigation 
provides location and time information in all weather conditions, anywhere on or near the earth but it requires an 
clear line of sight to at least four GPS satellites. 
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(a) Communication and exteroceptive sensing ranges (b) Communication graph 


Figure 2: This figure depicts a multi-hop communication scenario for the multi-robot team. Plot (a) shows the 
communication and measurement ranges. Here, robots 1 and 6 make relative measurements, respectively, of robots 
2 and 3. Plot (b) shows the communication graph generated using the communication ranges given in plot (a). Here 
the robot at the head of an arrow can send information to the robot at the tip of the arrow. As this graph shows, 
each of robots 1 and 6 can pass communication message to the entire team via a multi-hop strategy. 
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Figure 3: Estimation error (solid line) and Scr error bounds (dashed lines) in the x—coordinate variable for 3 robots 
moving on a flat terrain when they (a) only propagate their equations of motion using self-motion measurements 
(black plots), (b) employ cooperative localization ignoring past correlations between the estimations of the robots 
(blue plots), (c) employ cooperative localization with accurate account of past correlations (red plots). The figures 
on the right column are the same figures as on the left where the localization case (a) is removed for clearer 
demonstration of cases (b) and (c). Here, a —?► 5 over the time interval marked by two vertical blue lines indicates 
that robot a has taken a relative measurement with respect to robot b at that time interval. The symbol a ^ a 
means that robot a obtains an absolute measurement. 
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Robot 1 


Robot 2 


Robots 


Figure 4: Trajectories of the robots for the simulation study of Fig. 3 Here, the gray curve is the ground truth. 
The red curve is the estimation of the trajectory by implementing a EKF CL. The blue (resp. green) ellipses show 
the 95% uncertainty regions for the estimations at 2 seconds before (resp. after) any change in the measurement 
scenario (see Fig. ^ 


39 









- Correlation ^- Relative measurement ' Exteroceptive sensing zone 




Tightly coupled D-CL 


Figure 5: Schematic representation of the D-CL classification based on how the past correlations are accounted 
for. 
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Propagation stage for all k e {0,1,2, ■ ■ • } 

Update stage 


Figure 6: The in-network information flow of the Interim Master D-CL algorithm. In the Interim Master D-CL 
algorithm, communication is only needed in the update stage when the team members use a robot-to-robot relative 
measurement feedback to correct their pose estimation. Here, we assume that all the team members are in the 
communication range of the interim master robot. 
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Figure 7: A comparison study between the positioning accuracy of 3 robots employing the Interim Master D-CL 


algorithm (red plots), with that from the EKF Covariance-Intersection based CL algorithm of (dashed green 


plot). The curves in black show the positioning accuracy when the robots do not use any CL. As expected, the 


Interim Master D-CL algorithm by keeping an accurate account of the cross-covariances produces more accurate 


localization results than the algorithm of ll25l . However, this higher accuracy comes with higher communication 


and processing cost per robot. Notice here that using algorithm of ll25l robot 3 does not get to update its estimation 


equations. 
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Sidebar 1 


Further Reading 

A performance analysis of an EKF CL for a team of homogeneous robots moving on a flat terrain, 
with the same level of uncertainty in their proprioceptive measurements and exteroceptive sensors 
that measure relative pose, is provided in [ISTlI and [fS2ll . Interestingly, [ISTl shows that the rate 
of uncertainty growth decreases as the size of the robot team increases, but is subject to the law 
of diminishing returns. Moreover, llS3 shows that the upper bound on the rate of uncertainty 
growth is independent of the accuracy or the frequency of the robot-to-robot measurements. 
The consistency of EKF CL from the perspective of observability is studies in jSSl. Huang et 
al. in [IS3ll analytically show that the error-state system model employed in the standard EKF 
CL always has an observable subspace of higher dimension than that of the actual nonlinear CL 
system. This results in an unjustified reduction of the EKF covariance estimates in directions of 
the state space where no information is available, and thus leads to inconsistency. To address this 
problem, Huang et al. in [[S3ll adopt an observability-based methodology for designing consistent 
estimators in which the linearization points are selected to ensure a linearized system model with 
an observable subspace of the correct dimension. More results on observability analysis of CL 
can be found in fUt . [|S^ . llSSll . The use of an observability analysis to explicitly design an 
active local path planning algorithm for unmanned aerial vehicles implementing a bearing-only 
CL is discussed in llS^ . The necessity for an initialization procedure for CL is discussed in [ISTll . 
There it is shown that, because of system nonlinearities and the periodicity of the orientation, 
initialization errors can lead to erroneous results in covariance-based filters. An initialization 
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procedure for the state estimation in a CL seenario based on ranging and dead reekoning is 
studied in dSU. 
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